#!/usr/bin/env python
# -*- coding: UTF-8 -*-

import rospy
import numpy as np
import math
from std_msgs.msg import Bool
from geometry_msgs.msg import Twist
from sensor_msgs.msg import LaserScan

class rplidarDetector:
    def __init__(self):
        self.min_dist = 0
        self.has_obs = Bool()
        self.lidar_cmd = Twist()
        self.angleCoef = 2
        self.angleScale = -7
        self.threshold  = 0.9
        self.head_offset = 0.25

        rospy.init_node('obstacle_detection', anonymous=True)
        rospy.Subscriber('/scan', LaserScan, self.callback)
        self.pub = rospy.Publisher('has_obs', Bool, queue_size=1)
        self.cmdpub = rospy.Publisher('lidar_vel', Twist, queue_size=1)


    def callback(self, msg):
        sizeofLidar = 1440
        minIndexLeft_start = sizeofLidar * 1 / 4
        minIndexLeft_end  = sizeofLidar * 2 / 4
        minIndexRight_start = sizeofLidar * 2 / 4
        minIndexRight_end = sizeofLidar * 3 / 4
        minIndexLeft = 0
        minIndexRight = sizeofLidar/2
        distMinLeft = 10000
        distMinRight = 10000

        # goes throgh  array and find minimum  on the right 
        for i in range (minIndexLeft_start, minIndexLeft_end):
            theta_head2obs = (i-sizeofLidar/2) * msg.angle_increment
            dis = msg.ranges[i]
            dis_head2obs = math.sqrt((dis*math.cos(theta_head2obs)-self.head_offset)**2
                                     +(dis*math.sin(theta_head2obs))**2)
            if dis_head2obs < distMinLeft and dis_head2obs > 0.05:
                minIndexLeft = i
                distMinLeft = dis_head2obs
                
        for i in range (minIndexRight_start, minIndexRight_end):
            theta_head2obs = (i-sizeofLidar/2) * msg.angle_increment
            dis = msg.ranges[i]
            dis_head2obs = math.sqrt((dis*math.cos(theta_head2obs)-self.head_offset)**2
                                     +(dis*math.sin(theta_head2obs))**2)
            if dis_head2obs < distMinRight and dis_head2obs > 0.05:
                minIndexRight = i
                distMinRight = dis_head2obs
                
        
        if distMinLeft >= distMinRight: # TODO
            self.lidar_cmd.angular.z = self.angleCoef*distMinLeft/distMinRight-self.angleCoef
        else:
            self.lidar_cmd.angular.z = -(self.angleCoef*distMinRight/distMinLeft-self.angleCoef)
        self.lidar_cmd.angular.z *= self.angleScale
        if distMinLeft <= self.threshold and distMinRight <= self.threshold:
            self.has_obs = True
        else:
            self.has_obs = False
            
        #public message
        self.pub.publish(self.has_obs)
        self.cmdpub.publish(self.lidar_cmd)
        
if __name__ == '__main__':
    try:
        detector = rplidarDetector()
        rospy.spin()
    except rospy.ROSInterruptException:
        pass
